#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include "drone_defs.h"
#include <mavros_msgs/SetMode.h>


std::string flight_mode;
bool connected = false;
geometry_msgs::PoseStamped local_position;
geometry_msgs::PoseStamped set_position;
geometry_msgs::PoseStamped target_position;
ros::ServiceClient drone_mode_client;
bool is_stop_local = false;

void StateCallback(const mavproxy_ros1::MavproxyState::ConstPtr &msg)
{
    flight_mode = (*msg).flight_mode;
    connected = (*msg).connected;
}

void PositionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    local_position = (*msg);
}

void SetPointCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    set_position = (*msg);
}

void TargetPointCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    target_position = (*msg);
}

int SwitcheModeByName(const std::string &mode_name)
{
    mavros_msgs::SetMode srv;
    srv.request.custom_mode = mode_name;

    if (drone_mode_client.call(srv) && srv.response.mode_sent) {
        ROS_INFO("[GTR] Drone mode set to %s", mode_name.c_str());
        return RETSUCCESS;
    }

    ROS_ERROR("[GTR] Failed to set drone mode to %s", mode_name.c_str());
    return RETFAILED;
}

float GetDistance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
{
    float dx = p1.pose.position.x - p2.pose.position.x;
    float dy = p1.pose.position.y - p2.pose.position.y;
    float dz = p1.pose.position.z - p2.pose.position.z;
    return sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2));
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "goal_controller");
    ros::NodeHandle nh;

    ros::Subscriber mavproxy_state_sub = nh.subscribe<mavproxy_ros1::MavproxyState>(MAVPROXY_STATE, GLOBAL_DEFAULT_QUEUE_SIZE, StateCallback);
    ros::Subscriber local_positin_sub = nh.subscribe<geometry_msgs::PoseStamped>(MAVROS_INFO_LOCAL_POSE, GLOBAL_DEFAULT_QUEUE_SIZE, PositionCallback);
    ros::Subscriber set_position_sub = nh.subscribe<geometry_msgs::PoseStamped>(MAVPROXY_SET_POINT, GLOBAL_DEFAULT_QUEUE_SIZE, SetPointCallback);
    ros::Subscriber target_position_sub = nh.subscribe<geometry_msgs::PoseStamped>(MAVPROXY_GOAL, GLOBAL_DEFAULT_QUEUE_SIZE, TargetPointCallback);


    ros::Publisher set_position_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVROS_SET_LOCAL_POSITION, GLOBAL_DEFAULT_QUEUE_SIZE);
    ros::Publisher avoidance_goal_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVPROXY_GOAL, GLOBAL_DEFAULT_QUEUE_SIZE);

    drone_mode_client = nh.serviceClient<mavros_msgs::SetMode>(MAVROS_SRV_MODE);

    int ret = nh.getParam("/goal_controller/is_stop_local", is_stop_local);
    if (ret) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Get stop local parameter: %d", is_stop_local);
    }

    // 建立连接后等待3s
    ros::Rate rate(GLOBAL_COMMON_ROS_RATE / 2);
    ros::Time last_request = ros::Time::now();
    while (ros::ok() && !connected && ros::Time::now() - last_request < ros::Duration(DRONE_CONNECTION_TIMEOUT_DURATION))
    {
        ros::spinOnce();
        rate.sleep();
    }
    
    // 初始化发布目标
    target_position.pose.position.x = 0;
    target_position.pose.position.y = 0;
    target_position.pose.position.z = 2.5;

    ros::Rate keep_rate(GLOBAL_COMMON_ROS_RATE / 5);
    while (ros::ok())
    {
        // offboard持续计算是否到底目标点
        if ((flight_mode == OFFBOARD_PX4_MODE)) {
            float distance = GetDistance(local_position, target_position);
            if (distance < GLOBAL_REACHED_DISTANCE) {
                ROS_INFO("[TD DEBUG][GTR] Arrived at target position, switch mode to hold.");
		        SwitcheModeByName(HOLD_PX4_MODE);
            }
        }

        ros::spinOnce();
        keep_rate.sleep();

        if (flight_mode == HOLD_PX4_MODE && is_stop_local) {
            continue;
        }else{
            set_position_pub.publish(set_position);
        }
    }
}

